# 从大文件逐行读取并解析数据

import numpy as np
def load_lidar_data_parse_rpy(file_path):
    data = []  # 用于存储所有帧数据
    current_frame = {}  # 当前帧数据
    points = []  # 点云数据

    try:
        with open(file_path, 'r', encoding='utf-8', errors='ignore') as f:  # 忽略无效字符
            for line in f:  # 使用生成器逐行读取文件
                line = line.strip()

                try:
                    if line.startswith("time_stamp:"):
                        # 如果当前帧已经有数据，保存到 data 中
                        if current_frame:
                            current_frame['points'] = np.array(points)  # 将点云数据转为 NumPy 数组
                            data.append(current_frame)
                            current_frame = {}
                            points = []

                        # 解析时间戳
                        current_frame['time_stamp'] = int(line.split(":")[1].strip())

                    elif line.startswith("fusepose:"):
                        # 解析 fusepose
                        current_frame['fusepose'] = list(map(float, line.split(":")[1].strip().split()))

                    elif line.startswith("rpy:"):
                        # 解析 rpy，只取前三个数
                        rpy_values = line.split(":")[1].strip().split()
                        current_frame['rpy'] = list(map(float, rpy_values[:3]))  # 只取前三个数

                    elif line.startswith("pcl_no:"):
                        # 解析点云数量（可选，不一定需要用到）
                        current_frame['pcl_no'] = int(line.split(":")[1].strip())

                    else:
                        # 解析点云数据
                        if line:  # 确保不是空行
                            points.append(list(map(float, line.split())))

                except ValueError as e:
                    # 忽略无法解析的行
                    print(f"解析行失败，跳过：{line}，错误：{e}")

        # 保存最后一帧
        if current_frame:
            current_frame['points'] = np.array(points)
            data.append(current_frame)

    except Exception as e:
        print(f"读取文件失败: {e}")

    return data
